// --------------------------------------------------------------------------------------------------------------------
// <copyright file="VirtualDriveFeedbackTests.cs" company="Microsoft Corporation">
// The MIT License (MIT)
// 
// Copyright (c) 2014, Microsoft Corporation
// 
// Permission is hereby granted, free of charge, to any person obtaining a copy
//  of this software and associated documentation files (the "Software"), to deal
//  in the Software without restriction, including without limitation the rights
//  to use, copy, modify, merge, publish, distribute, sublicense, and/or sell
//  copies of the Software, and to permit persons to whom the Software is
//  furnished to do so, subject to the following conditions:
// 
// The above copyright notice and this permission notice shall be included in
//  all copies or substantial portions of the Software.
// 
// THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR
//  IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY,
//  FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL THE
//  AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER
//  LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING FROM,
//  OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN
//  THE SOFTWARE.
// </copyright>
// --------------------------------------------------------------------------------------------------------------------
namespace Internal.Robotics.Tools.VirtualDriveFeedback
{
    using System;
    using System.Collections.Generic;
    using System.Diagnostics;
    using System.Reflection;
    using Microsoft.Robotics.Hardware.DAC;
    using Microsoft.Robotics.Hardware.Runtime.DAC;
    using Microsoft.Robotics.Navigation.Perception;
    using Microsoft.Robotics.Navigation.Runtime.Perception;
    using Microsoft.Robotics.Numerics;
    using Microsoft.Robotics.Runtime;
    using Microsoft.VisualStudio.TestTools.UnitTesting;

    /// <summary>
    /// VirtualDriveFeedbackAgent tests.
    /// </summary>
    [TestClass]
    public class VirtualDriveFeedbackTests
    {
        /// <summary>
        /// Error epsilon
        /// </summary>
        private const double Epsilon = 0.001;

        /// <summary>
        /// Starting state.
        /// </summary>
        private static DriveFeedbackState startingState = new DriveFeedbackState(0, 0, 1, new OdometryNoiseModelParams(), null);

        /// <summary>
        /// Test that gyro integration over time works.
        /// </summary>
        [Priority(0)]
        [TestMethod]
        [TestCategory("Unit")]
        public void GyroHeadingDeltaTest()
        {
            VirtualDriveFeedback virtualDriveFeedback = new VirtualDriveFeedback(
                gyroCalibrationThreshold: 1,
                driftThresholdRadiansPerSecond: 0,
                slipThresholdPercentage: 0,
                gyroYCalibrationFactor: 1,
                requiredGyroMeasurementsForCalibration: 1);

            // first sample, no delta
            Assert.AreEqual(
                0,
                virtualDriveFeedback.GyroHeadingDelta(
                    GyroscopeMessageWithTimestamp(
                        new GyroscopeState() { RotationalVelocityInRadiansPerSecond = new Vector3(0, 0, 0), }, 100)));

            // second sample (from 0 to 5 radians/sec in 100ms)
            double headingDelta = virtualDriveFeedback.GyroHeadingDelta(
                GyroscopeMessageWithTimestamp(new GyroscopeState() { RotationalVelocityInRadiansPerSecond = new Vector3(0, 5, 0) }, 200));

            Assert.AreEqual(
                .25,
                headingDelta,
               Epsilon);

            // third sample (5 radians/s for 50ms)
            headingDelta = virtualDriveFeedback.GyroHeadingDelta(
                GyroscopeMessageWithTimestamp(new GyroscopeState() { RotationalVelocityInRadiansPerSecond = new Vector3(0, 5, 0) }, 250));

            Assert.AreEqual(
                .25,
                headingDelta,
                Epsilon);
        }

        /// <summary>
        /// Test that wheel position based heading deltas work.
        /// </summary>
        [Priority(0)]
        [TestMethod]
        [TestCategory("Unit")]
        public void OdometryHeadingDeltaTest()
        {
            VirtualDriveFeedback agent = new VirtualDriveFeedback(
                gyroCalibrationThreshold: 1,
                driftThresholdRadiansPerSecond: 0,
                slipThresholdPercentage: 0,
                gyroYCalibrationFactor: 1,
                requiredGyroMeasurementsForCalibration: 1);

            DriveFeedbackState driveFeedbackState = new DriveFeedbackState(0, 0, 1, new OdometryNoiseModelParams(), null);

            Pose2D odometryDelta = agent.GetOdometryDelta(driveFeedbackState);
            DriveFeedbackState odoDriveFeedbackState = new DriveFeedbackState(-1, 1, 1, new OdometryNoiseModelParams(), driveFeedbackState);
            DriveFeedbackState updatedDriveFeedbackState = agent.GetUpdatedDriveFeedbackState(odometryDelta.Heading, odometryDelta.Heading, odoDriveFeedbackState);
            
            Pose2D updatedPose = updatedDriveFeedbackState.GetMeanDeltaPose(driveFeedbackState);
            Assert.AreEqual(updatedPose, new Pose2D(0, 0, 2));
        }

        /// <summary>
        /// Test virtual drive feedback with no slip/drift (all odometry).
        /// </summary>
        [Priority(0)]
        [TestMethod]
        [TestCategory("Unit")]
        public void VirtualDriveFeedbackAgentWithNoSlipDriftTest()
        {
            DriveFeedbackState[] states = RunScenario(
                new VirtualDriveFeedback(
                    gyroCalibrationThreshold: 1,
                    driftThresholdRadiansPerSecond: 1000,
                    slipThresholdPercentage: 1000,
                    gyroYCalibrationFactor: 1,
                    requiredGyroMeasurementsForCalibration: 1));

            this.AssertEqual(states[0], 0, 0, 100, 0);
            this.AssertEqual(states[1], 2, 2, 54.030230586814, 84.1470984807897);
            this.AssertEqual(states[2], 2, 2, 54.030230586814, 84.1470984807897);
            this.AssertEqual(states[3], -1.06192982974676, -1.06192982974676, 87.0942061010561, -51.1429297520856);
        }

        /// <summary>
        /// Test virtual drive feedback with all slip/drift (no odometry).
        /// </summary>
        [Priority(0)]
        [TestMethod]
        [TestCategory("Unit")]
        public void VirtualDriveFeedbackAgentWithAllSlipDriftTest()
        {
            DriveFeedbackState[] states = RunScenario(
                new VirtualDriveFeedback(
                    gyroCalibrationThreshold: 99,
                    driftThresholdRadiansPerSecond: 0.0000001,
                    slipThresholdPercentage: 0,
                    gyroYCalibrationFactor: 1,
                    requiredGyroMeasurementsForCalibration: 1));  

            this.AssertEqual(states[0], 0, 0, 100, 0);
            this.AssertEqual(states[1], 2, 2, 54.030230586814, 84.1470984807897);
            this.AssertEqual(states[2], 2, 2, 54.030230586814, 84.1470984807897);
            this.AssertEqual(states[3], -1.06192982974676, -1.06192982974676, 87.0942061010561, -51.1429297520856);
        }

        /// <summary>
        /// Test virtual drive feedback with all slip, but no drift.
        /// </summary>
        [Priority(0)]
        [TestMethod]
        [TestCategory("Unit")]
        public void VirtualDriveFeedbackAgentWithAllSlipNoDriftTest()
        {
            DriveFeedbackState[] states = RunScenario(
                new VirtualDriveFeedback(
                    gyroCalibrationThreshold: 1,
                    driftThresholdRadiansPerSecond: 1000,
                    slipThresholdPercentage: 0,
                    gyroYCalibrationFactor: 1,
                    requiredGyroMeasurementsForCalibration: 1));
            this.AssertEqual(states[0], 0, 0, 100, 0);
            this.AssertEqual(states[1], 2, 2, 54.030230586814, 84.1470984807897);
            this.AssertEqual(states[2], 2, 2, 54.030230586814, 84.1470984807897);
            this.AssertEqual(states[3], -1.06192982974676, -1.06192982974676, 87.0942061010561, -51.1429297520856);
        }

        /// <summary>
        /// Test virtual drive feedback with all slip, but no drift.
        /// </summary>
        [Priority(0)]
        [TestMethod]
        [TestCategory("Unit")]
        public void VirtualDriveFeedbackAgentWithNoSlipAllDriftTest()
        {
            DriveFeedbackState[] states = RunScenario(
                new VirtualDriveFeedback(
                    gyroCalibrationThreshold: 99,
                    driftThresholdRadiansPerSecond: 0.0000001,
                    slipThresholdPercentage: 0,
                    gyroYCalibrationFactor: 1,
                    requiredGyroMeasurementsForCalibration: 1));

            this.AssertEqual(states[0], 0, 0, 100, 0);
            this.AssertEqual(states[1], 2, 2, 54.030230586814, 84.1470984807897);
            this.AssertEqual(states[2], 2, 2, 54.030230586814, 84.1470984807897);
            this.AssertEqual(states[3], -1.06192982974676, -1.06192982974676, 87.0942061010561, -51.1429297520856);
        }

        /// <summary>
        /// Run scenario of several wheel and gyro updates.
        /// </summary>
        /// <param name="virtualDriveFeedback">Agent to which updates are sent.</param>
        /// <returns>Series of resulting drive feedback states.</returns>
        private static DriveFeedbackState[] RunScenario(VirtualDriveFeedback virtualDriveFeedback)
        {
            // no motion
            DriveFeedbackState driveFeedback0 = new DriveFeedbackState(100, 100, 1, new OdometryNoiseModelParams(), startingState);
              
            double driveDelta0 = virtualDriveFeedback.GetOdometryDelta(driveFeedback0).Heading;
            double gyroDelta0 =
                virtualDriveFeedback.GyroHeadingDelta(
                    GyroscopeMessageWithTimestamp(
                        new GyroscopeState() { RotationalVelocityInRadiansPerSecond = new Vector3(0, 0, 0) }, 100));
            DriveFeedbackState state0 = virtualDriveFeedback.GetUpdatedDriveFeedbackState(
                driveDelta0, gyroDelta0, driveFeedback0);

            // wheels turn less than gyro
            DriveFeedbackState driveFeedback1 = new DriveFeedbackState(99, 101, 1, new OdometryNoiseModelParams(), startingState);
            double driveDelta1 = virtualDriveFeedback.GetOdometryDelta(driveFeedback1).Heading;
            double gyroDelta1 =
                virtualDriveFeedback.GyroHeadingDelta(
                    GyroscopeMessageWithTimestamp(
                        new GyroscopeState() { RotationalVelocityInRadiansPerSecond = new Vector3(0, 1, 0) }, 200));
            DriveFeedbackState state1 = virtualDriveFeedback.GetUpdatedDriveFeedbackState(
                driveDelta1, gyroDelta1, driveFeedback1);

            // wheels stopped, but gyro moving
            DriveFeedbackState driveFeedback2 = new DriveFeedbackState(99, 101, 1, new OdometryNoiseModelParams(), startingState);
            double driveDelta2 = virtualDriveFeedback.GetOdometryDelta(driveFeedback2).Heading;
            double gyroDelta2 =
                virtualDriveFeedback.GyroHeadingDelta(
                    GyroscopeMessageWithTimestamp(
                        new GyroscopeState() { RotationalVelocityInRadiansPerSecond = new Vector3(0, 1, 0) }, 300));
            DriveFeedbackState state2 = virtualDriveFeedback.GetUpdatedDriveFeedbackState(
                driveDelta2, gyroDelta2, driveFeedback2);

            // wheels turn more than gyro
            DriveFeedbackState driveFeedback3 = new DriveFeedbackState(1, 201, 1, new OdometryNoiseModelParams(), startingState);
            double driveDelta3 = virtualDriveFeedback.GetOdometryDelta(driveFeedback3).Heading;
            double gyroDelta3 =
                virtualDriveFeedback.GyroHeadingDelta(
                    GyroscopeMessageWithTimestamp(
                        new GyroscopeState() { RotationalVelocityInRadiansPerSecond = new Vector3(0, 1, 0) }, 400));
            DriveFeedbackState state3 = virtualDriveFeedback.GetUpdatedDriveFeedbackState(
                driveDelta3, gyroDelta3, driveFeedback3);

            return new[] { state0, state1, state2, state3 };
        }

        /// <summary>
        /// Construct Gyroscope with private timestamp field set via reflection.
        /// </summary>
        /// <param name="state">IMUState as message body.</param>
        /// <param name="millis">Timestamp in milliseconds.</param>
        /// <returns>The gyroscope message</returns>
        private static GyroscopeMessage GyroscopeMessageWithTimestamp(GyroscopeState state, int millis)
        {
            GyroscopeMessage message = new GyroscopeMessage(new List<GyroscopeState>(new[] { state }), AgentMessage.GetCurrentTime());
            typeof(AgentMessage).GetField("originatingTime", BindingFlags.Instance | BindingFlags.NonPublic).SetValue(
                message, millis);
            return message;
        }

        /// <summary>
        /// Assert drive feedback state fields.
        /// </summary>
        /// <param name="state">Drive feedback state.</param>
        /// <param name="absHeading">Absolute drive heading estimate.</param>
        /// <param name="heading">Drive heading estimate.</param>
        /// <param name="posX">Drive position (x).</param>
        /// <param name="posY">Drive position (y).</param>
        private void AssertEqual(DriveFeedbackState state, double absHeading, double heading, double posX, double posY)
        {            
            Pose2D updatedPose = state.GetMeanDeltaPose(startingState);
            Trace.WriteLine(string.Format("{0}, {1}, {2}, {3}", updatedPose.Heading, updatedPose.Heading, updatedPose.X, updatedPose.Y));
            Assert.AreEqual(updatedPose.Heading, heading, Epsilon);
            Assert.AreEqual(updatedPose.X, posX, Epsilon);
            Assert.AreEqual(updatedPose.Y, posY, Epsilon);
        }
    }
}
